#ifndef VISUAL_LOCATION_CAMERAPARAMS_H
#define VISUAL_LOCATION_CAMERAPARAMS_H

#include "rapidjson/document.h"
#include <iostream>
#include "utils.h"
#include "Eigen/Core"
#include "RapidjsonUtils.h"

class CameraParams {
public:
	/**
	 * 内参以毫米为单位
	 */
	double cam0_px{}, cam0_py{}, cam0_fx{}, cam0_fy{}, cam1_px{}, cam1_py{}, cam1_fx{}, cam1_fy{};
	double cam0_ppm_x, cam0_ppm_y, cam1_ppm_x, cam1_ppm_y;
	Eigen::Vector3d cam0_intrinsic, cam1_intrinsic;
	Eigen::Vector3d baseline;
};

class StereoParams : public CameraParams {
private:
	void _handleConfig(rapidjson::Value &dom) {
		rapidjson::Value &leftCam = getObjectFrom(dom, "left");
		rapidjson::Value &rightCam = getObjectFrom(dom, "right");
		rapidjson::Value &baselineVec = getObjectFrom(dom, "baseline");
		double cam0_sx_mm = getDoubleFrom(leftCam, "sensor_width");
		double cam0_sy_mm = getDoubleFrom(leftCam, "sensor_height");
		double cam0_cols = getIntegerFrom(leftCam, "cols");
		double cam0_rows = getIntegerFrom(leftCam, "rows");
		this->cam0_ppm_x = cam0_cols / cam0_sx_mm;
		this->cam0_ppm_y = cam0_rows / cam0_sy_mm;

		this->cam0_px = getDoubleFrom(leftCam, "px") / cam0_ppm_x;
		this->cam0_py = getDoubleFrom(leftCam, "py") / cam0_ppm_y;
		this->cam0_fx = getDoubleFrom(leftCam, "fx") / cam0_ppm_x;
		this->cam0_fy = getDoubleFrom(leftCam, "fy") / cam0_ppm_y;

		double cam1_sx_mm = getDoubleFrom(rightCam, "sensor_width");
		double cam1_sy_mm = getDoubleFrom(rightCam, "sensor_height");
		double cam1_cols = getIntegerFrom(rightCam, "cols");
		double cam1_rows = getIntegerFrom(rightCam, "rows");
		this->cam1_ppm_x = cam1_cols / cam1_sx_mm;
		this->cam1_ppm_y = cam1_rows / cam1_sy_mm;

		this->cam1_px = getDoubleFrom(rightCam, "px") / cam1_ppm_x;
		this->cam1_py = getDoubleFrom(rightCam, "py") / cam1_ppm_y;
		this->cam1_fx = getDoubleFrom(rightCam, "fx") / cam1_ppm_x;
		this->cam1_fy = getDoubleFrom(rightCam, "fy") / cam1_ppm_y;

		this->cam0_intrinsic << cam0_px, cam0_py, (cam0_fx + cam0_fy) * 0.5f;
		this->cam1_intrinsic << cam1_px, cam1_py, (cam1_fx + cam1_fy) * 0.5f;
		this->baseline << getDoubleFrom(baselineVec, "x"), getDoubleFrom(baselineVec, "y"), getDoubleFrom(baselineVec, "z");
	}

public:
	explicit StereoParams(rapidjson::Value &rapidCfg) {
		_handleConfig(rapidCfg);
	}

	explicit StereoParams(const std::string &configFile) {
		rapidjson::Document dom = getDOM(configFile);
		_handleConfig(dom);
	}
};



class MonoParams : public CameraParams {
private:
	void _handleConfig(rapidjson::Value &cam) {
		this->cam0_px = getDoubleFrom(cam, "px");
		this->cam0_py = getDoubleFrom(cam, "py");
		double fx_mm = getDoubleFrom(cam, "fx");
		double fy_mm = getDoubleFrom(cam, "fy");
		double sx_mm = getDoubleFrom(cam, "sensor_width");
		double sy_mm = getDoubleFrom(cam, "sensor_height");
		int cols = getIntegerFrom(cam, "cols");
		int rows = getIntegerFrom(cam, "rows");
		double ppm_x = cols*1.0 / sx_mm;
		double ppm_y = rows*1.0 / sy_mm;
		this->cam0_fx = ppm_x * fx_mm;
		this->cam0_fy = ppm_y * fy_mm;
		this->cam0_intrinsic << cam0_px, cam0_py, (cam0_fx + cam0_fy) * 0.5f;
	}

public:
	explicit MonoParams(rapidjson::Value &cam) {
		_handleConfig(cam);
	}

	explicit MonoParams(const std::string &configFile) {
		rapidjson::Document dom = getDOM(configFile);
		_handleConfig(dom);
	}
};


#endif //VISUAL_LOCATION_CAMERAPARAMS_H
